using System;
using System.Collections.Generic;
using System.Text;
using System.Runtime.InteropServices;

namespace OpenVCWrapper
{
    public class OpenCVWrapper
    {
        [StructLayout(LayoutKind.Explicit)]
        public class CvMat
        {
            [FieldOffset(0)]
            public int type;
            [FieldOffset(4)]
            public int step;
            [FieldOffset(8)]
            public IntPtr refcount;

            [FieldOffset(12)]
            public IntPtr db;
            //union
            //{
            //uchar* ptr;
            //short* s;
            //int* i;
            //float* fl;
            //double* db;
            //} data; /* data pointers */


            [FieldOffset(16)]
            public int rows; /* number of rows */
            [FieldOffset(20)]
            public int cols; /* number of columns */

        }

        [DllImport("cxcore097.dll")]
        private static extern IntPtr cvCreateMat(int rows, int cols, int type);

        [DllImport("cxcore097.dll")]
        private static extern void cvReleaseMat(ref IntPtr mat);

        [DllImport("cxcore097.dll")]
        private static extern double cvDet(CvMat mat);

        [DllImport("cxcore097.dll")]
        private static extern CvMat cvCloneMat(CvMat mat);

        [DllImport("cxcore097.dll")]
        public static extern void cvSetData(CvMat arr, IntPtr data, int step);

        //void cvFindExtrinsicCameraParams2( const CvMat* object_points,
        //                           const CvMat* image_points,
        //                           const CvMat* intrinsic_matrix,
        //                           const CvMat* distortion_coeffs,
        //                           CvMat* rotation_vector,
        //                           CvMat* translation_vector );

        [DllImport("cv097.dll")]
        private static extern void cvFindExtrinsicCameraParams2(CvMat object_points,
                               CvMat image_points,
                               CvMat intrinsic_matrix,
                               CvMat distortion_coeffs,
                               CvMat rotation_vector,
                               CvMat translation_vector);

        [DllImport("cv097.dll")]
        private static extern int cvRodrigues2(CvMat src, CvMat dst, CvMat jacobian);

        [DllImport("OpenCVNativeWrapper.dll", EntryPoint = "FindExtrinsicCameraParams")]
        private static extern void cvFindExtrinsicCameraParams(
        int N,
        double[] object_points,    //3N
        double[] image_points,     //2N
        double[] intrinsic_matrix, //9
        double[] distortion_coeffs,//4
        double[] rotation_matrix,  //9
        double[] translation_vector);//3


        public const int CV_8U = 0;
        public const int CV_8S = 1;
        public const int CV_16U = 2;
        public const int CV_16S = 3;
        public const int CV_32S = 4;
        public const int CV_32F = 5;
        public const int CV_64F = 6;
        public const int CV_USRTYPE1 = 7;

        public static void FindExtrinsicCameraParams2(double[,] objectPoints,
                               double[,] imagePoints,
                               double[,] intrinsicMatrix,
                               double[,] distortionCoeffs,
                               out double[] translationVector,
                               out double[,] rodrigues2
            )
        {

            double[] rotationVector;
            IntPtr p1 = cvCreateMat(objectPoints.GetLength(0), objectPoints.GetLength(1), CV_64F);
            IntPtr p2 = cvCreateMat(imagePoints.GetLength(0), imagePoints.GetLength(1), CV_64F);
            IntPtr p3 = cvCreateMat(intrinsicMatrix.GetLength(0), intrinsicMatrix.GetLength(1), CV_64F);
            IntPtr p4 = cvCreateMat(distortionCoeffs.GetLength(0), distortionCoeffs.GetLength(1), CV_64F);
            IntPtr p5 = cvCreateMat(1, 3, CV_64F);
            IntPtr p6 = cvCreateMat(1, 3, CV_64F);

            CvMat object_points = new CvMat();
            CvMat image_points = new CvMat();
            CvMat intrinsic_matrix = new CvMat();
            CvMat distortion_coeffs = new CvMat();
            CvMat rotation_vector = new CvMat();
            CvMat translation_vector = new CvMat();

            Marshal.PtrToStructure(p1, object_points);
            Marshal.PtrToStructure(p2, image_points);
            Marshal.PtrToStructure(p3, intrinsic_matrix);
            Marshal.PtrToStructure(p4, distortion_coeffs);
            Marshal.PtrToStructure(p5, rotation_vector);
            Marshal.PtrToStructure(p6, translation_vector);

            double[] objectPoints1D = new double[objectPoints.GetLength(0) * objectPoints.GetLength(1)];
            for (int i = 0; i < objectPoints1D.Length; i++)
            {
                objectPoints1D[i] = objectPoints[i / objectPoints.GetLength(1), i % objectPoints.GetLength(1)];
            }

            SetCvMatrix(object_points, objectPoints1D);

            double[] imagePoints1D = new double[imagePoints.GetLength(0) * imagePoints.GetLength(1)];
            for (int i = 0; i < imagePoints1D.Length; i++)
            {
                imagePoints1D[i] = imagePoints[i / imagePoints.GetLength(1), i % imagePoints.GetLength(1)];
            }


            SetCvMatrix(image_points, imagePoints1D);

            double[] intrinsicMatrix1D = new double[intrinsicMatrix.GetLength(0) * intrinsicMatrix.GetLength(1)];
            for (int i = 0; i < intrinsicMatrix1D.Length; i++)
            {
                intrinsicMatrix1D[i] = intrinsicMatrix[i / intrinsicMatrix.GetLength(1), i % intrinsicMatrix.GetLength(1)];
            }


            SetCvMatrix(intrinsic_matrix, intrinsicMatrix1D);

            double[] distortionCoeffs1D = new double[distortionCoeffs.GetLength(0) * distortionCoeffs.GetLength(1)];
            for (int i = 0; i < distortionCoeffs1D.Length; i++)
            {
                distortionCoeffs1D[i] = distortionCoeffs[i / distortionCoeffs.GetLength(1), i % distortionCoeffs.GetLength(1)];
            }

            SetCvMatrix(distortion_coeffs, distortionCoeffs1D);

            cvFindExtrinsicCameraParams2(object_points, image_points, intrinsic_matrix, distortion_coeffs, rotation_vector, translation_vector);

            rotationVector = new double[rotation_vector.cols];
            translationVector = new double[translation_vector.cols];

            Marshal.Copy(rotation_vector.db, rotationVector, 0, rotationVector.Length);
            Marshal.Copy(translation_vector.db, translationVector, 0, translationVector.Length);


            IntPtr p7 = cvCreateMat(3, 3, CV_64F);
            CvMat rodrigues2Mat = new CvMat();
            Marshal.PtrToStructure(p7, rodrigues2Mat);


            cvRodrigues2(rotation_vector, rodrigues2Mat, null);
            double[] rodrigues21D = new double[rodrigues2Mat.cols * rodrigues2Mat.rows];

            Marshal.Copy(rodrigues2Mat.db, rodrigues21D, 0, rodrigues21D.Length);
            rodrigues2 = new double[rodrigues2Mat.rows, rodrigues2Mat.cols];
            for (int i = 0; i < rodrigues21D.Length; i++)
            {
                rodrigues2[i / rodrigues2Mat.cols, i % rodrigues2Mat.cols] = rodrigues21D[i];
            }

            // GCHandle.FromIntPtr(p1).Free();
            //GCHandle.FromIntPtr(p2).Free();
            //GCHandle.FromIntPtr(p3).Free();
            //GCHandle.FromIntPtr(p4).Free();
            //GCHandle.FromIntPtr(p5).Free();
            //GCHandle.FromIntPtr(p6).Free();
            //GCHandle.FromIntPtr(p7).Free();

            //Marshal.DestroyStructure(object_points.db);
            //Marshal.DestroyStructure(image_points.db);
            //Marshal.DestroyStructure(intrinsic_matrix.db);
            //Marshal.DestroyStructure(distortion_coeffs.db);
            //Marshal.DestroyStructure(rotation_vector.db);
            //Marshal.DestroyStructure(translation_vector.db);
            //Marshal.DestroyStructure(rodrigues2Mat.db);

            //Marshal.Release(p1);
            //Marshal.Release(p2);
            //Marshal.Release(p3);
            //Marshal.Release(p4);
            //Marshal.Release(p5);
            //Marshal.Release(p6);
            //Marshal.Release(p7);

            cvReleaseMat(ref p1);
            cvReleaseMat(ref p2);
            cvReleaseMat(ref p3);
            cvReleaseMat(ref p4);
            cvReleaseMat(ref p5);
            cvReleaseMat(ref p6);
            cvReleaseMat(ref p7);


        }

        private static unsafe void SetCvMatrix(CvMat mat, double[] data)
        {
            void* p = mat.db.ToPointer();
            double* d = (double*)p;
            for (int i = 0; i < data.Length; i++)
            {
                (*(d + i)) = data[i];
            }

        }


        public static void FindExtrinsicCameraParams(double[,] objectPoints,
                               double[,] imagePoints,
                               double[,] intrinsicMatrix,
                               double[,] distortionCoeffs,
                               out double[] translationVector,
                               out double[,] rodrigues2
            )
        {
            double[] op = ConvertToArray(objectPoints);
            double[] ip = ConvertToArray(imagePoints);
            double[] im = ConvertToArray(intrinsicMatrix);
            double[] dc = ConvertToArray(distortionCoeffs);
            double[] rm = new double[9];
            translationVector = new double[3];

        cvFindExtrinsicCameraParams(op.Length / 3, op, ip, im, dc, rm, translationVector);

            rodrigues2 = new double[3, 3];
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    rodrigues2[i, j] = rm[i * 3 + j];
                }
            }
        }

        private static double[] ConvertToArray(double[,] data)
        {
            double[] ret = new double[data.GetLength(0) * data.GetLength(1)];
            for (int i = 0; i < ret.Length; i++)
            {
                ret[i] = data[i / data.GetLength(1), i % data.GetLength(1)];
            }
            return ret;
        }


    }
}
